/*
 * Application\Src\Library\GSensor\LSM303DLH.c
 *
 * LSM303DLH function
 *
 * Author: Fenris Liang
 * Copyright (c) 2010
 * All rights reserved.
 *
 * Modifications:
 * 2010/12/08 Fenris Liang	first implemetation
 *
 * $Id: LSM303DLH.c 2010-12-08 09:32:52 Fenris Liang $
 */

#include "LSM303DLH.h"
#include "stm32f10x.h"
#include "i2c.h"
#include "tool.h"
#include <stdlib.h>
#include <stdio.h>
#include "memory.h"
#include "Console.h"
#include "string.h"

#define LSM303DLH_IDA		0x28
#define LSM303DLH_IDB		0x34
#define LSM303DLH_IDC		0x33

#define LSM303DLH_SAD_A_W	0x30
#define LSM303DLH_SAD_A_R	0x31
#define LSM303DLH_SAD_M_W   0x3C
#define LSM303DLH_SAD_M_R   0x3D

#define MULTI_RW			0x80


#define Ctrl_Reg1_A					0x20
#define Ctrl_Reg2_A					0x21
#define Ctrl_Reg3_A					0x22
#define Ctrl_Reg4_A					0x23
#define Ctrl_Reg5_A					0x24
#define HP_Filter_Reset_A		0x25
#define Reference_A					0x26
#define Status_Reg_A				0x27
#define Out_X_L_A						0x28
#define Out_X_H_A						0x29
#define Out_Y_L_A						0x2A
#define Out_Y_H_A						0x2B
#define Out_Z_L_A						0x2C
#define Out_Z_H_A						0x2D
#define INT1_CFG_A					0x30
#define INT1_SOURCE_A				0x31
#define INT1_THS_A					0x32
#define INT1_DURATION_A			0x33
#define INT2_CFG_A					0x34
#define INT2_SOURCE_A				0x35
#define INT2_THS_A					0x36
#define INT2_DURATION_A			0x37

#define CRA_Reg_M						0x00
#define CRB_Reg_M						0x01
#define MR_Reg_M						0x02
#define Out_X_H_M						0x03
#define Out_X_L_M						0x04
#define Out_Y_H_M						0x05
#define Out_Y_L_M						0x06
#define Out_Z_H_M						0x07
#define Out_Z_L_M						0x08
#define SR_Reg_Mg						0x09
#define IRA_Reg_M						0x0A
#define IRB_Reg_M						0x0B
#define IRC_Reg_M						0x0C


typedef struct _LIS_INIT_TBL{
	unsigned char reg;
	unsigned char value;
}LIS_INIT_TBL;

const LIS_INIT_TBL LSM303DLH_Init_Tbl_A[] =
{
	//A
	{ Ctrl_Reg1_A,			0x2F },
	{ Ctrl_Reg2_A,			0x0F },
	{ Ctrl_Reg3_A,			0x27 },
	{ Ctrl_Reg4_A,			0x00 },
	{ Ctrl_Reg5_A,			0x03 },
	{ INT1_CFG_A,			0x2A },
	{ INT1_THS_A,			0x7F },
	{ INT1_DURATION_A, 		0x32 },
	{ INT2_CFG_A,			0x2A },
	{ INT2_THS_A,			0x7F },
	{ INT2_DURATION_A,		0x32 },
};

const LIS_INIT_TBL LSM303DLH_Init_Tbl_M[] =
{
	//M
    { CRA_Reg_M,				0x10 },
	{ CRB_Reg_M,				0x60 },
	{ MR_Reg_M,                 0x00 },
};

//fenris 20110126  ==start==
const LIS_INIT_TBL LSM303DLH_Init_Tbl_M_Set[] =
{
	//M
    { MR_Reg_M,                 0x03 },
    { CRA_Reg_M,				0x00 },
    { CRB_Reg_M,                0x00 },
    { CRA_Reg_M,				0x10 },
	{ CRB_Reg_M,				0x60 },
	{ MR_Reg_M,                 0x00 },
    { MR_Reg_M,                 0x03 },
    { CRA_Reg_M,				0x11 },
	{ MR_Reg_M,                 0x00 },
};
const LIS_INIT_TBL LSM303DLH_Init_Tbl_M_Reset[] =
{
	//M
    { MR_Reg_M,                 0x03 },
    { CRA_Reg_M,				0x12 },
	{ MR_Reg_M,                 0x00 },
};
const LIS_INIT_TBL LSM303DLH_Init_Tbl_M_Normal[] =
{
	//M
    { MR_Reg_M,                 0x03 },
    { CRA_Reg_M,				0x10 },
	{ MR_Reg_M,                 0x00 },
};
const LIS_INIT_TBL LSM303DLH_Init_Tbl_M_Self_Test_Set[] =
{
	//M
    { MR_Reg_M,                 0x03 },
    { CRA_Reg_M,				0x00 },
    { CRB_Reg_M,                0x00 },
    { CRA_Reg_M,				0x10 },
	{ CRB_Reg_M,				0x20 },
	{ MR_Reg_M,                 0x00 },
    { CRB_Reg_M,                0x00 },
    { CRA_Reg_M,				0x11 },
	{ CRB_Reg_M,				0x20 },
	{ MR_Reg_M,                 0x01 },
};
//fenris 20110126  ===end===

static int LSM303DLH_Read_A(unsigned char reg, char *buf, unsigned char len)
{
	if (len > 1)
		reg |= MULTI_RW;
	
	return I2C_Read(LSM303DLH_SAD_A_R, reg, (unsigned char *)buf, len);
}

static int LSM303DLH_Write_A(unsigned char reg, char *buf, unsigned char len)
{
	if (len > 1)
		reg |= MULTI_RW;
	
	return I2C_Write(LSM303DLH_SAD_A_W, reg, (unsigned char *)buf, len);
}

static int LSM303DLH_Read_M(unsigned char reg, unsigned char *buf, unsigned char len)
{
	if (len > 1)
		reg |= MULTI_RW;
	
	return I2C_Read(LSM303DLH_SAD_M_R, reg, (unsigned char *)buf, len);
}

/************
static int LSM303DLH_Write_M(unsigned char reg, char *buf, unsigned char len)
{
	if (len > 1)
		reg |= MULTI_RW;
	
	return I2C_Write(LSM303DLH_SAD_M_W, reg, (unsigned char *)buf, len);
}
*************/

int LSM303DLH_Init(void)
{
	unsigned char id;
	int i;
	
	I2C_HW_Init();
    	
	for (i = 0; i < (sizeof(LSM303DLH_Init_Tbl_A)/sizeof(LIS_INIT_TBL)); i++)
	{
		I2C_Write(LSM303DLH_SAD_A_W, LSM303DLH_Init_Tbl_A[i].reg, (unsigned char *)&LSM303DLH_Init_Tbl_A[i].value, 1);
	}
	
	I2C_Read(LSM303DLH_SAD_A_R, INT1_SOURCE_A, &id, 1);
	I2C_Read(LSM303DLH_SAD_A_R, INT2_SOURCE_A, &id, 1);
    
    for (i = 0; i < (sizeof(LSM303DLH_Init_Tbl_M)/sizeof(LIS_INIT_TBL)); i++)
	{
		I2C_Write(LSM303DLH_SAD_M_W, LSM303DLH_Init_Tbl_M[i].reg, (unsigned char *)&LSM303DLH_Init_Tbl_M[i].value, 1);
	}

	return 0;
}

//fenris 20110126 ==start==
void LSM303DLH_M_Set(int idx)
{
    I2C_Write(LSM303DLH_SAD_M_W, LSM303DLH_Init_Tbl_M_Set[idx].reg, (unsigned char *)&LSM303DLH_Init_Tbl_M_Set[idx].value, 1);
}

void LSM303DLH_M_Reset(int idx)
{
    I2C_Write(LSM303DLH_SAD_M_W, LSM303DLH_Init_Tbl_M_Reset[idx].reg, (unsigned char *)&LSM303DLH_Init_Tbl_M_Reset[idx].value, 1);
}

void LSM303DLH_M_Normal(int idx)
{
    I2C_Write(LSM303DLH_SAD_M_W, LSM303DLH_Init_Tbl_M_Normal[idx].reg, (unsigned char *)&LSM303DLH_Init_Tbl_M_Normal[idx].value, 1);
}

void LSM303LH_M_Self_Test_Set(int idx)
{
    I2C_Write(LSM303DLH_SAD_M_W, LSM303DLH_Init_Tbl_M_Self_Test_Set[idx].reg, (unsigned char *)&LSM303DLH_Init_Tbl_M_Self_Test_Set[idx].value, 1);
}

void LSM303LH_M_Self_Test_Reset(void)
{
    for (int i = 1; i < (sizeof(LSM303DLH_Init_Tbl_M_Reset)/sizeof(LIS_INIT_TBL)); i++)
	{
		I2C_Write(LSM303DLH_SAD_M_W, LSM303DLH_Init_Tbl_M_Reset[i].reg, (unsigned char *)&LSM303DLH_Init_Tbl_M_Reset[i].value, 1);
	}
}
//fenris 20110126 ===end===

void LSM303DLH_GET_XYZ_A(short *arg)
{
	char buf[7];
	
	LSM303DLH_Read_A(Status_Reg_A, buf, 7);
	arg[0] = (((short)buf[2]) << 8) + buf[1];
	arg[1] = (((short)buf[4]) << 8) + buf[3];
	arg[2] = (((short)buf[6]) << 8) + buf[5];
}

void LSM303DLH_INT1_SOURCE_A(char *arg)
{
	char th[2];
	
	th[0] = arg[0] | 0x80;
	th[1] = arg[1];
	
	LSM303DLH_Write_A(INT1_THS_A, th, 2);
}

void LSM303DLH_INT2_SOURCE_A(char *arg)
{
	char th[2];
	
	th[0] = arg[0] | 0x80;
	th[1] = arg[1];
	LSM303DLH_Write_A(INT2_THS_A, th, 2);
}

void LSM303DLH_GET_EVENT_A(char *arg)
{
	char buf;
			
	LSM303DLH_Read_A(INT1_SOURCE_A, &buf, 1);
	arg[0] = buf;
	LSM303DLH_Read_A(INT2_SOURCE_A, &buf, 1);
	arg[1] = buf;
}

void LSM303DLH_Sleep(void)
{
	char buf;
			
	LSM303DLH_Read_A(Ctrl_Reg1_A, &buf, 1);
	buf &= 0x1F; 
	LSM303DLH_Write_A(Ctrl_Reg1_A, &buf, 1);
}

void LSM303DLH_Wakeup(void)
{
	char buf;
			
	LSM303DLH_Read_A(Ctrl_Reg1_A, &buf, 1);
	buf |= 0x20; 
	LSM303DLH_Write_A(Ctrl_Reg1_A, &buf, 1);
}

void LSM303DLH_GET_XYZ_M(short *arg)
{
	unsigned char buf[7];
	
	LSM303DLH_Read_M(MR_Reg_M, buf, 7);
	arg[0] = (short)(((unsigned short)buf[1]) << 8) + (unsigned short)buf[2];
	arg[1] = (short)(((unsigned short)buf[3]) << 8) + (unsigned short)buf[4];
	arg[2] = (short)(((unsigned short)buf[5]) << 8) + (unsigned short)buf[6];
}
